/* EasyCODE V8 */
/* EasyCODE ( 0 */
#include <p33FJ16GS504.h>
#include "bootloader_init.h"
#include "../../dsESC_Bram.X/Source/init.h"

_FOSCSEL(FNOSC_PRIPLL)
_FOSC(POSCMD_XT & OSCIOFNC_ON)
_FPOR(FPWRT_PWR128 )
_FICD(ICS_PGD3 & JTAGEN_OFF)
_FWDT(FWDTEN_OFF)


/* EasyCODE E */
/* EasyCODE F */
void boot_delay()
{
    volatile long j = 1;
    while (j++ != 0)
    {
        ;
    }
}

void boot_waiti2c()
{
    I2C1CONbits.SCLREL = 1; // always release clock.
    while (IFS1bits.SI2C1IF == 0);
    IFS1bits.SI2C1IF = 0;
}

unsigned char boot_i2cget()
{
    boot_waiti2c();
    return I2C1RCV;
}

uReg32 boot_i2cget3()
{
    uReg32 a;
    a.Val[0] = boot_i2cget();
    a.Val[1] = boot_i2cget();
    a.Val[2] = boot_i2cget();
    return a;
}

char boot_i2cwait_transmit()
{
    boot_waiti2c();
    // read address
    if (I2C1STATbits.R_W != 1 || I2C1STATbits.D_A != 0) return 0;
    I2C1RCV;
    return 1;
}

void boot_i2cput_wait(unsigned char b)
{
    I2C1TRN = b;
    I2C1CONbits.SCLREL = 1;
    boot_waiti2c();
}

//char Buffer[PM_ROW_SIZE*3+1];
/* EasyCODE ( 0
__attribute__ */
/* EasyCODE F */
int __attribute__((keep,naked,noreturn)) main()
{
//    asm("mov #__SP_init,w15"); // initialize w15
//    asm("mov #__SPLIM_init,w0"); // ;
//    asm("mov w0,_SPLIM"); // ; initialize SPLIM")
//    asm("nop");

    uReg32 gotoBootloader;
    uReg32 configVars[64];

    CORCONbits.PSV = 1;

    boot_initclock();
    boot_initport();
    boot_init_comm();

    gotoBootloader = boot_getflashword(0);

    {
        int i;
        for (i=0;i<64;i++)
        {
            configVars[i] = boot_getflashword(0x200+i*2);
        }
    }
    GLED = 1;
    WLED = 1;
    BLED = 1;

    while (1)
    {
         // release clock
        I2C1CONbits.I2CEN = 0;
        I2C1CONbits.SCLREL = 1;
        I2C1CONbits.I2CEN = 1;
        boot_waiti2c();
        unsigned char rcv = I2C1RCV;

        // only parse i2c requests that start with write address
        if (I2C1STATbits.R_W != 0) { I2C1TRN = 0xFF; I2C1CONbits.SCLREL = 1; continue; }
        if (I2C1STATbits.D_A != 0) { continue; }
        // only parse i2c requests with command bit
        if ((rcv & 0x80) != 0x80) {  continue; }
        I2C1CONbits.SCLREL = 1;
        boot_waiti2c();
        rcv = I2C1RCV;
        if (rcv == 0xFE)
        {
            if (boot_i2cwait_transmit() == 0) continue;

            // bootloader is on (=0x01)
            boot_i2cput_wait(0x01);
            continue;
        }
        // reset command
        else if (rcv == 0xFD)
        {
            I2C1CONbits.SCLREL = 1;
            I2C1CONbits.STREN = 0;
            asm("GOTO 0x802");
            while(1);
        }
        else if (rcv == 0xF0)
        {
            if (boot_i2cwait_transmit() == 0) continue;

            uReg32 val;

            boot_i2cput_wait(0x01); // bootloader

            val = boot_getflashword(0x800);
            boot_i2cput_wait(val.Val[1]);
            boot_i2cput_wait(val.Val[0]);

            val = boot_getflashword(0xFF0000);
            boot_i2cput_wait(val.Val[1]);
            boot_i2cput_wait(val.Val[0]);

            val = boot_getflashword(0xFF0002);
            boot_i2cput_wait(val.Val[1]);
            boot_i2cput_wait(val.Val[0]);
            continue;
        }
        else if (rcv == 0x62) // write single row
        {
            uReg32 value, source;
            char    Size;
            source = boot_i2cget3();
            //I2C1CONbits.STREN = 1;
            I2C1CONbits.SCLREL = 0;

            TBLPAG = ((uReg32)source).Word.HW;

            if ((source.Val32 & 0x3FF) == 0 && source.Val32 != 0x400) boot_erasepage(source.Val32);
                //boot_delay();
            I2C1CONbits.SCLREL = 1;

            for (Size = 0; Size < PM_ROW_SIZE; Size++)
            {
                value = boot_i2cget3();
                value.Val[3]=0;

                if (source.Val32 == 0) // load goto bootloader command..
                    value = gotoBootloader;
                else if (source.Val32 >= 0x200 && source.Val32 < 0x200 + 64*2)
                    value = configVars[(source.Word.LW - 0x200) / 2];

                if (source.Val32 != 0x400)
                {
                    // Write Latch
                    boot_setflashword(source.Val32, value.Val32);

                    source.Val32 += 2;
                }
            }
            if (source.Val32 != 0x400)
            {
                boot_writerow();
            }
                //boot_delay();
        }
        else if (rcv == 0x63) // write config.
        {
            uReg32 value, source;
            source = boot_i2cget3();

            value = boot_i2cget3();

            TBLPAG = source.Word.HW;
            boot_setflashword(source.Val32, value.Val32);

            NVMCON = CONFIG_WORD_WRITE;
            __builtin_write_NVM();
            while (NVMCONbits.WR);
            NVMCONbits.WREN = 0;
        }
        else if (rcv == 0x64) // read word
        {
            uReg32 value, source;
            source = boot_i2cget3();
            value = boot_getflashword(source.Val32);
            if (boot_i2cwait_transmit() == 0) continue;
            boot_i2cput_wait(value.Val[0]);
            boot_i2cput_wait(value.Val[1]);
            boot_i2cput_wait(value.Val[2]);
            asm("NOP");
            continue;
        }
        else
        {
            continue;
        }

        if (boot_i2cwait_transmit() == 0) continue;

        boot_i2cput_wait(NVMCONbits.WRERR == 0 ? 0xA5 : 0x5A);
    }
}

/* EasyCODE ) */
/* EasyCODE ) */
